package drone;

import com.jme3.asset.AssetManager;
import com.jme3.material.Material;
import com.jme3.math.Vector3f;
import com.jme3.scene.Geometry;
import com.jme3.scene.shape.Sphere;
import com.jme3.texture.Texture;
import java.util.ArrayList;

/**
 * VERWEILDAUER BZW SIGNAL, DASS DROHNE LOSFLIEGT UND AUFTRAG ABARBEITET
 * @author Kjell Bunjes
 */
public class Drone extends Geometry {
    
    private AssetManager assetManager;
    private Drone reference;
    private float x, y, z;
    private float destX, destY, destZ;
    private float droneAngle, destinationAngle;
    private float rotationDelta;
    private Vector3f normDirVector;
    private Coordinates startingPoint, rechargePoint;
    private Coordinates gate;
    private boolean isDroneStarting, isFacingDestination, isDroneLanding, isTaskComplete, isCharging, isGuiding;
    private float altitude;
    private float velocity;
    private float acceleration;
    private final float TOLERANCE = 0.05f;
    private final float ROTATION_DEFAULT = 0.35f;
    private final float ALTITUDE_DEFAULT = 4f;
    private final float VELOCITY_DEFAULT = 3f;
    private final float ACCELERATION_DEFAULT = 1f;
    private final float ACCELERATION_MIN = 0.5f;
    private final float ACCELERATION_MAX = 5f;   
    private final float ACCELERATION_INCREASE = 0.00075f;
    private final float ACCELERATION_DECREASE = -0.0003f;
    private ArrayList<Coordinates> taskList;
    
    public Drone(AssetManager assetManager) {
        super();
        this.assetManager = assetManager;
        setDefaults();
        printSetup();
    }
    
    private void setDefaults() {
        taskList = new ArrayList<Coordinates>();        
        altitude = ALTITUDE_DEFAULT;
        velocity = VELOCITY_DEFAULT;
        acceleration = ACCELERATION_DEFAULT;
        isDroneStarting = false;
        isFacingDestination = false;
        isDroneLanding = false;
        isTaskComplete = true;
        isCharging = true;
        isGuiding = false;
        droneAngle = 0;
        y = 0;
        setAppearance();
    }
    
    private void setAppearance() {        
        Sphere sphereMesh = new Sphere(25, 25, 0.5f);
        setMesh(sphereMesh);
        Material sphereMat = new Material(assetManager, "Common/MatDefs/Misc/Unshaded.j3md");
        Texture sphereTex = assetManager.loadTexture("/Interface/texDrone.jpg");
        sphereMat.setTexture("ColorMap", sphereTex);
        setMaterial(sphereMat);
    }
    
    private void printSetup() {
       /* System.out.println("- - - - - - Drone set to the following settings - - - - - -");
        System.out.println("Tolerance: " + TOLERANCE);
        System.out.println("Acceleration default value: " + ACCELERATION_DEFAULT);
        System.out.println("Minimum acceleration: " + ACCELERATION_MIN + " maximum acceleration: " + ACCELERATION_MAX);
        System.out.println("Acceleration increases " + (ACCELERATION_INCREASE * 100) + "% per iteration");
        System.out.println("Acceleration decreases " + (ACCELERATION_DECREASE * (-100)) + "% per iteration");
        System.out.println("Drone's altitude is set to: " + ALTITUDE_DEFAULT);
        System.out.println("Drone's velocity is set to: " + VELOCITY_DEFAULT);*/
    }
    
    public void update(float tpf) {        
         // If list of destination points still has items, the drone is still acttive
        if(!taskList.isEmpty()) {
            fetchNextTask();
            if(y < altitude && isDroneLanding == false) {
                // Single information at the start
                if(isDroneStarting == false) {
                    isDroneStarting = true;
                    System.out.println("Drone started at x: " + x + " y: " + y + " z: " + z);
                    for(int i = 0; i < taskList.size(); i++) {
                        System.out.println("x: " + taskList.get(i).getxPos() + " z: " + taskList.get(i).getzPos());
                    }
                }
                moveToAltitude(tpf);
            }
            // Drone is ready to move to destination point            
            else {               
               if((x != destX || z != destZ) && isDroneLanding == false) {
                   if(isTaskComplete == true) {
                       System.out.println("Drone acquired new task with destination at x: " + destX + " y: " + destY + " z: " + destZ);
                       isTaskComplete = false;
                       computeOrientation();
                       
                       //
                       if((destX != startingPoint.getxPos() && destZ != startingPoint.getzPos()) && (destX != rechargePoint.getxPos() && destZ != rechargePoint.getzPos())) {
                           isGuiding = true;
                       }
                       else {
                           isGuiding = false;
                       }
                       
                       System.out.println("isGuiding: " + isGuiding);
                       //
                   }
                   if(isFacingDestination == false) {
                       orientateToDestination(tpf);
                       lookAt(new Vector3f(destX, altitude, destZ), new Vector3f(0, 1, 0));
                       isFacingDestination = true;
                   }
                   else {
                       moveToDestination(tpf);
                   }                   
                }
                // Drone is ready to land
                else {
                    land(tpf);
                    //System.out.println("altitude: " + altitude + " y: " + y + " isDroneLanding: " + isDroneLanding);
                }
            }
        }
    }
    
    private void moveToAltitude(float tpf) {
        // Making sure that the drone reaches the exact altitude
        if(y + TOLERANCE >= altitude || y - TOLERANCE >= altitude) {
            y = altitude;
            setLocalTranslation(x, y, z);
            acceleration = ACCELERATION_DEFAULT;
            System.out.println("Altitude of: " + altitude + " is reached!");
        }
        else {
            if(acceleration < ACCELERATION_MAX) {
                acceleration += acceleration * ACCELERATION_INCREASE;
            }
            move(0, acceleration * velocity * tpf, 0);
            y = getLocalTranslation().y;
        }
    }
    
    private void orientateToDestination(float tpf) {
        
        /*if(droneAngle + ROTATION_DEFAULT * tpf < destinationAngle) {
            rotate(0, ROTATION_DEFAULT * tpf, 0);
            droneAngle += ROTATION_DEFAULT * tpf;
            System.out.println("Drone's angle: " + droneAngle + " destination angle: " + destinationAngle);
        }
        else {
            System.out.println("Done!");
            isFacingDestination = true;
            droneAngle = 0;
        }*/
        
        /*float w = getLocalRotation().getW();
        
        if(rotationDelta > 0) {
            getLocalRotation().set(x, y, z, w + ROTATION_DEFAULT * tpf);
            rotationDelta -= ROTATION_DEFAULT * tpf;
            System.out.println("Orientating to positive direction!");
            if(rotationDelta <= 0) {
                rotationDelta = 0;
                isFacingDestination = true;
                System.out.println("Facing target now!");
            }
        }
        else if(rotationDelta < 0){
            getLocalRotation().set(x, y, z, w - ROTATION_DEFAULT * tpf);
            rotationDelta += ROTATION_DEFAULT * tpf;
            System.out.println("Orientating to negative direction!");
            if(rotationDelta >= 0) {
                rotationDelta = 0;
                isFacingDestination = true;
                System.out.println("Facing target now!");
            }
        }
        
        System.out.println("RotationDelta: " + rotationDelta);*/
    }
    
    private void moveToDestination(float tpf) {
        // Making sure that the drone reaches the exact destination point
        if(((x + TOLERANCE > destX || x - TOLERANCE > destX) && x - TOLERANCE < destX) && ((z + TOLERANCE > destZ || z - TOLERANCE > destZ) && z - TOLERANCE < destZ)) {
            x = destX;
            z = destZ;
            setLocalTranslation(x, y, z);
            acceleration = ACCELERATION_DEFAULT;
            // Marking current task as complete and setting next task as current task
            if(taskList.size() > 0) {
                System.out.println("Destination reached!");
                if(taskList.size() > 1) {
                    taskList.remove(0);
                    isFacingDestination = false;
                    fetchNextTask();
                    isTaskComplete = true;
                }
            }
        }
        else {
            if(acceleration < ACCELERATION_MAX) {
                acceleration += acceleration * ACCELERATION_INCREASE;
            }
            move(acceleration * velocity * normDirVector.x * tpf, 0, acceleration * velocity * normDirVector.z * tpf);
            x = getLocalTranslation().x;
            z = getLocalTranslation().z;
        }   
    }
    
    private void fetchNextTask() {        
       Coordinates destination = taskList.get(0);
       setDestinationPoint(destination.getxPos(), destination.getzPos());
        // Computing the directional vector for the new destination point
        Vector3f dirVector = getDestinationPoint().subtract(getStartingPoint());
        // Computing the length of the directional vector
        float dirVectorLength = (float) Math.sqrt(Math.pow(dirVector.x, 2) + Math.pow(dirVector.z, 2));
        // Normalizing the directional vector in order to move the drone in the direction with a constant speed
        normDirVector  = new Vector3f(dirVector.x/dirVectorLength, dirVector.y, dirVector.z/dirVectorLength);
    }
    
    /**
     * Needs work, rotation isn't computing properly. Degrees above PI / 180° get resetted by default.
     * This needs a workaround!
     */
    private void computeOrientation() {
        System.out.println("orientating");
        
        /*reference = new Drone(assetManager);
        reference.setLocalRotation(getLocalRotation());
        
        reference.lookAt(new Vector3f(destX, altitude, destZ), new Vector3f(0, 1, 0));
        
        rotationDelta = reference.getLocalRotation().getW() - getLocalRotation().getW() - reference.getLocalRotation().getW();
        
        //lookAt(new Vector3f(destX, altitude, destZ), new Vector3f(0, 1, 0));
        
        System.out.println("Rotation difference alpha: " + rotationDelta);
        
        
        /*
        System.out.println(getLocalRotation().fromRotationMatrix(Matrix3f.ZERO).getRotationColumn(0));
        System.out.println(getLocalRotation().fromRotationMatrix(Matrix3f.ZERO).getRotationColumn(1));
        System.out.println(getLocalRotation().fromRotationMatrix(Matrix3f.ZERO).getRotationColumn(2));
        
        
        float vectorLength = (float) Math.sqrt(Math.pow(getDestinationPoint().x, 2) + Math.pow(getDestinationPoint().z, 2));
        
        Vector3f normVector  = new Vector3f(getDestinationPoint().x/vectorLength, getDestinationPoint().y, getDestinationPoint().z/vectorLength);        
        destinationAngle = normVector.angleBetween(new Vector3f(1, 0, 0));
        
        if((normVector.x < 0 && normVector.z > 0) || (normVector.x > 0 && normVector.z > 0)) {
            destinationAngle = (float) ((Math.PI / 2) - destinationAngle);
        }
        System.out.println("Destination angle: " + destinationAngle);
        System.out.println(getStartingPoint());
        Vector3f point1 = new Vector3f(5, 0, -5);
        Vector3f point2 = new Vector3f(-5, 0, -5);
        Vector3f point3 = new Vector3f(-5, 0, 5);
        Vector3f point4 = new Vector3f(5, 0, 5);
        
        float length1 = (float) Math.sqrt(Math.pow(point1.x, 2) + Math.pow(point1.z, 2));
        Vector3f norm1 = new Vector3f(point1.x/length1, point1.y, point1.z/length1);
        
        float length2 = (float) Math.sqrt(Math.pow(point2.x, 2) + Math.pow(point2.z, 2));
        Vector3f norm2 = new Vector3f(point2.x/length2, point2.y, point2.z/length2);
        
        float length3 = (float) Math.sqrt(Math.pow(point3.x, 2) + Math.pow(point3.z, 2));
        Vector3f norm3 = new Vector3f(point3.x/length3, point3.y, point3.z/length3);
        
        float length4 = (float) Math.sqrt(Math.pow(point4.x, 2) + Math.pow(point4.z, 2));
        Vector3f norm4 = new Vector3f(point4.x/length4, point4.y, point4.z/length4);
        
        System.out.println("Quadrant 1: " + norm1.angleBetween(new Vector3f(1, 0, 0)));
        System.out.println("Quadrant 2: " + norm2.angleBetween(new Vector3f(1, 0, 0)));
        System.out.println("Quadrant 3: " + norm3.angleBetween(new Vector3f(1, 0, 0)));
        System.out.println("Quadrant 4: " + norm4.angleBetween(new Vector3f(1, 0, 0)));
        
        
        
        System.out.println("dir: " + getLocalRotation().getW());*/
    }
    
    private void land(float tpf) {
        if(y > 0) {
            if(isDroneLanding == false) {
                System.out.println("Landing drone!");
                isDroneLanding = true;
            }
            if(y - TOLERANCE <= 0) {
                y = 0;
                setLocalTranslation(x, 0, z);
                taskList.remove(0);
                isDroneLanding = false;
                System.out.println("- - - - - - Drone landed and fulfilled all tasks successfully - - - - - -");
                if(x == rechargePoint.getxPos() && z == rechargePoint.getzPos()) {
                    System.out.println("Recharging");
                    isCharging = true;
                }
            }
            else {
                if(acceleration > ACCELERATION_MIN) {
                    acceleration += acceleration * ACCELERATION_DECREASE;
                }
                move(0, velocity * -tpf  * acceleration, 0);
                y = getLocalTranslation().y;
            }                        
        }
    }
    
    public void setStartingPoint(Coordinates startingPoint) {
        this.startingPoint = startingPoint;
    }
    
    public void setPosition(Coordinates position) {
        this.x = position.getxPos();
        this.z = position.getzPos();
        setLocalTranslation(position.getxPos(), y, position.getzPos());
        
    }
    
    public void setRechargePoint(Coordinates rechargePoint) {
        this.rechargePoint = rechargePoint;
    }
    
    private void setDestinationPoint(float x, float z) {
        destX = x;
        destY = 0f;
        destZ = z;    
    }
    
    public void setRoute(ArrayList<Coordinates> route) {
        for(Coordinates destinationPoint : route) {
            taskList.add(destinationPoint);
        }
        // Setting first point in the list as first destination
        fetchNextTask();
    }
    
    private Vector3f getDestinationPoint() {
        Vector3f destinationPoint = new Vector3f(destX, destY, destZ);
        return destinationPoint;
    }
    
    public Vector3f getStartingPoint() {
        Vector3f startingVector = new Vector3f(x, y, z);
        return startingVector;
    }
    /*
     * Gini
     */
    public Coordinates getGate() {
        return gate;
    }
    public void setGate(Coordinates gate){
        this.gate = gate;
    }
    //
    
    public Vector3f getDronePosition() {
        return new Vector3f(x, y, z);
    }
    
    public void addTask(ArrayList<Coordinates> route) {
        System.out.println("New task added! Current size: " + taskList.size());
        if(taskList.isEmpty()) {
            if(isCharging) {
                taskList.add(startingPoint);
                isCharging = false;                
            }
            for(Coordinates points : route) {
                taskList.add(points);
            }
            taskList.add(rechargePoint);
        }
        else {
          taskList.remove(taskList.indexOf(rechargePoint));
          taskList.add(startingPoint);
          for(Coordinates points : route) {
              taskList.add(points);
          }           
           taskList.add(rechargePoint);
        }
        //System.out.println("taskList afterwards: " + taskList.size());
        for(int i = 0; i < taskList.size(); i++) {
            System.out.println("Waypoint: (" + taskList.get(i).getxPos() + " | " + taskList.get(i).getzPos() + ")");
        }
    }
    
    public boolean isDroneGuiding() {
        return isGuiding;
    }
}